package roboSoccer;

/**
 * A simple vector class, for representing the positions and directions of
 * things. Note that direction does NOT refer to an angle created by by the x
 * and y of the vector, direction is simply for storing the heading of things
 * like robots. For the angle created by x and y, use the angle method.
 * 
 * @author Ben Hopkins
 * 
 */
public class Vector {

	/**
	 * The x and y coordinates of this vector.
	 */
	public float x, y;

	/**
	 * The direction the object with this vector is facing, if any.
	 */
	public float direction;

	/**
	 * The zero vector.
	 */
	public static final Vector Zero = new Vector();
	
	/**
	 * Initializes components to zero.
	 */
	public Vector() {
		x = 0;
		y = 0;
		direction = 0;
	}

	/**
	 * Initializes the x and y components of the vector and leaves direction as
	 * zero.
	 * 
	 * @param x
	 * @param y
	 */
	public Vector(float x, float y) {
		this.x = x;
		this.y = y;
		direction = 0;
	}

	/**
	 * Initializes all vector components.
	 * 
	 * @param x
	 * @param y
	 * @param direction
	 */
	public Vector(float x, float y, float direction) {
		this.x = x;
		this.y = y;
		this.direction = direction;
	}
	
	/**
	 * Creates a vector with the given angle and length.
	 * 
	 * @param angle Angle in degrees from zero.
	 * @param length The resulting length of the vector.
	 * @param takeDirection Whether or not to set the direction to angle. Really just to
	 * 						differentiate this constructor from the other (float, float).
	 */
	public Vector(float angle, float length, boolean takeDirection)
	{
		x = (float) Math.cos(angle*Math.PI/180) * length;
		// make y component negative because the coordinate system has y increasing from the top down.
		y = (float) Math.sin(angle*Math.PI/180) * -length;
		if(takeDirection)
			direction = angle;
		else
			direction = 0;
	}
	
	/**
	 * For cloning a new vector object from this one.
	 */
	public Vector clone()
	{
		return new Vector(x,y,direction);
	}

	/**
	 * Returns a string with all the vector's component values.
	 */
	public String toString() {
		return "(" + x + ", " + y + ", " + direction + ")";
	}
	
	/**
	 * Returns a string of a the vector in the correct format to print for the simulation.
	 */
	public String printString()
	{
		return (int)x + " " + (int)y + " " + (int)direction;
	}

	/**
	 * Adds a vector to this vector, returns this vector (the result).
	 * 
	 * @param b
	 *            The vector added to the current vector.
	 * @return The resulting vector.
	 */
	public Vector add(Vector b) {
		Vector r = clone();
		r.x += b.x;
		r.y += b.y;
		return r;
	}

	/**
	 * Static method to add any two vectors.
	 * 
	 * @param a
	 *            The first vector.
	 * @param b
	 *            The second vector.
	 * @return The result from adding the two vectors.
	 */
	public static Vector add(Vector a, Vector b) {
		float x = a.x + b.x;
		float y = a.y + b.y;
		return new Vector(x, y, 0);
	}

	/**
	 * Subtracts a vector from this vector, returns this vector (the result).
	 * 
	 * @param b
	 *            Vector to subtract from this vector.
	 * @return The result after the subtraction.
	 */
	public Vector subtract(Vector b) {
		Vector r = clone();
		r.x -= b.x;
		r.y -= b.y;
		return r;
	}

	/**
	 * Static method to subtract one vector from another.
	 * 
	 * @param a
	 *            The first vector.
	 * @param b
	 *            Vector to subtract from the first.
	 * @return The result from the subtraction.
	 */
	public static Vector subtract(Vector a, Vector b) {
		float x = a.x - b.x;
		float y = a.y - b.y;
		return new Vector(x, y, 0);
	}

	/**
	 * Finds the length of this vector.
	 * 
	 * @return Distance as a float.
	 */
	public float distance() {
		return (float) Math.sqrt(Math.pow((double) x, 2)
				+ Math.pow((double) y, 2));
	}

	/**
	 * Finds the distance between the endpoints of this vector and another.
	 * 
	 * @param b
	 *            Vector to compare position to.
	 * @return Distance as a float.
	 */
	public float distance(Vector b) {
		return (float) Math.sqrt(Math.pow((x - b.x), 2)
				+ Math.pow((y - b.y), 2));
	}
	
	/**
	 * Gets the length of this vector.
	 * @return Float of the length.
	 */
	public float length()
	{
		return (float) Math.sqrt(Math.pow(x,2) + Math.pow(y, 2));
	}

	/**
	 * The angle in degrees that this vector makes with the normal.
	 * 
	 * @return Angle in degrees (0-360)
	 */
	public float vectorAngle() {
		return (float) (Math.atan2(y, x) * -180 / Math.PI + 180);
	}

	/**
	 * The angle between two vectors.
	 * 
	 * @param b
	 *            The other vector.
	 * @return Angle in degrees (-180 to 180)
	 */
	public float vectorAngle(Vector b) {
		return (float) (Math.atan2(y - b.y, x - b.x) * -180 / Math.PI + 180);
	}

	/**
	 * Finds the difference between two directions stored in vectors, not the
	 * difference of angles formed by vectors.
	 * 
	 * @param c
	 *            The other vector.
	 * @return Difference between the directions in the two vectors.
	 */
	public float directionAngleDifference(Vector c) {
		float a = direction;
		float b = c.direction;

		a = (a - b) % 360;
		if (a > 180)
			a = a - 360;
		return a;
	}

	/**
	 * Extends this vector a length in the direction it is going.
	 * 
	 * @param length
	 *            The length with which to extend the vector.
	 */
	public Vector extend(float length) {
		Vector r = clone();
		float angle = (float) Math.atan2(y, x);
		r.x = x + (float) Math.cos(angle) * length;
		r.y = y + (float) Math.sin(angle) * length;
		return r;
	}

	/**
	 * Truncates a vector to a certain length. Can be used to set the vector to any length.
	 * 
	 * @param length
	 *            The length to tuncate the vector to.
	 */
	public Vector truncate(float length) {
		Vector r = clone();
		float angle = (float) Math.atan2(y, x);
		r.x = (float) Math.cos(angle) * length;
		r.y = (float) Math.sin(angle) * length;
		return r;
	}

	/**
	 * Returns a vector opposite of this one.
	 * 
	 * @return The opposite vector.
	 */
	public Vector oppositeVector() {
		return new Vector(y * -1, x * -1, (direction + 180) % 360);
	}
	
	/**
	 * Multiplies the components of this vector by a number.
	 * @param n The number to multiply each component by.
	 */
	public Vector multiply(float n)
	{
		Vector r = clone();
		r.x *= n;
		r.y *= n;
		return r;
	}
	
	/**
	 * Divides the components of this vector by a number.
	 * @param n The number to divide each component by.
	 */
	public Vector divide(float n)
	{
		Vector r = clone();
		r.x /= n;
		r.y /= n;
		return r;
	}
	
	/**
	 * Good for getting things unstuck or turbulence.
	 * @param amount The maximum amount the vector's x and y components may change.
	 * @return A clone of this vector with shifted x and y components.
	 */
	public Vector randomShift(float amount)
	{
		Vector r = clone();
		r.x += Math.random()*amount*2 - amount;
		r.y += Math.random()*amount*2 - amount;
		return r;
	}

}
